﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace ArtemisTest
{
    public class QT<T>
    {
        const int MAX_CAPACITY = 1;
        AABB _bounds;
        List<xy> _points;
        List<T> _dataList;
        List<long> _uids;
        List<QT<T>> _children;

        public QT(AABB bounds)
        {
            _bounds = bounds;
            _children = new List<QT<T>>();
            _points = new List<xy>();
            _dataList = new List<T>();
            _uids = new List<long>();
        }

        public bool Insert(xy p, T data, long uid)
        {
            if (_bounds.ContainsPoint(p) == false) { return false; }
            if (_points.Count <= MAX_CAPACITY)
            {
                _points.Add(p);
                _dataList.Add(data);
                _uids.Add(uid);
                return true;
            }
            if (_children.Count == 0) { Subdivide(); }
            foreach (QT<T> t in _children)
            {
                if (t.Insert(p,data,uid)) { return true; }
            }
            return false;
        }

        public void Subdivide()
        {
            float size = _bounds._halfdimension._x;
            xy s = new xy(size, size);
            QT<T> topleft = new QT<T>(
                new AABB(new xy(_bounds._center._x - _bounds._halfdimension._x,
                                _bounds._center._y - _bounds._halfdimension._y),
                                s));

            QT<T> topRight = new QT<T>(
             new AABB(new xy(_bounds._center._x + _bounds._halfdimension._x,
                    _bounds._center._y - _bounds._halfdimension._y),
                    s));

            QT<T> bottomleft = new QT<T>(
            new AABB(new xy(_bounds._center._x - _bounds._halfdimension._x,
                    _bounds._center._y + _bounds._halfdimension._y),
                    s));

            QT<T> bottomRight = new QT<T>(
            new AABB(new xy(_bounds._center._x + _bounds._halfdimension._x,
                    _bounds._center._y + _bounds._halfdimension._y),
                    s));
            _children.Add(topleft);
            _children.Add(topRight);
            _children.Add(bottomleft);
            _children.Add(bottomRight);
        }

        public struct QTData
        {
            public T data;
            public bool found;
            public xy indexedPos;
            public long uid;
        }

        public bool UpdatePosition(xy pos, long uid)
        {
            QTData d = TakeData(uid, true);
            Insert(pos, d.data, uid);
            return d.found;
        }

        public QTData TakeData(long uid, bool removeFromTree)
        {
            QTData result = new QTData { data = default(T), found = false };

            if (_uids.Count == 0) { return result; }

            for (int i = 0; i < _uids.Count; i++)
            {
                if (_uids[i] == uid)
                {
                    result = new QTData { data = _dataList[i], found = true, indexedPos = _points[i]};
                    if (removeFromTree)
                    {
                        _dataList.RemoveAt(i);
                        _points.RemoveAt(i);
                        _uids.RemoveAt(i);
                    }
                }
            }

            if (result.found == true) { return result; }
            else
            {
                for (int i = 0; i < 4; i++)
                {
                    QTData childResult = _children[i].TakeData(uid,removeFromTree);
                    if (childResult.found) { return childResult; }
                }
                return result;
            }
        }

        //public T GetPointWithUID(long uid, out xy outPoint,out bool found)
        //{
        //    int outIndex = -1;
        //    T data = default(T);

        //    if (_uids[0] == uid) { outIndex = 0; }
        //    if (_uids[1] == uid) { outIndex = 1; }
        //    if (_uids[2] == uid) { outIndex = 2; }
        //    if (_uids[3] == uid) { outIndex = 3; }

        //    T[] datas = new T[4];

        //    if (outIndex == -1) //if didnt find a point with uid
        //    {
        //        datas[0] = _children[0].GetPointWithUID(uid, out outPoint, out found);
        //        datas[1] = _children[1].GetPointWithUID(uid, out outPoint, out found);
        //        datas[2] = _children[2].GetPointWithUID(uid, out outPoint, out found);
        //        datas[3] = _children[3].GetPointWithUID(uid, out outPoint, out found);

        //        for (int i = 0; i < 4; i++)
        //        {
        //            if (datas[i] != null)
        //            {
        //                data = datas[i];
        //                found = true;
        //            }
        //        }
        //    }
        //    else
        //    {
        //        data = _dataList[outIndex];
        //        outPoint = _points[outIndex];
        //        found = true;
        //        return data;
        //    }


        //}


        public List<Tuple<T,long>> GetAllData()
        {
            List<Tuple<T, long>> inRange = new List<Tuple<T, long>>();

            for (int i = 0; i < _points.Count; i++)
            {
                inRange.Add(new Tuple<T, long>(_dataList[i], _uids[i]));
            }

            if (_children.Count == 0) { return inRange; }

            inRange.AddRange(_children[0].GetAllData());
            inRange.AddRange(_children[1].GetAllData());
            inRange.AddRange(_children[2].GetAllData());
            inRange.AddRange(_children[3].GetAllData());

            return inRange;
        }

        public List<QTData> GetPointsInRange(AABB range, long nonInclusiveId = -1)
        {
            bool excludeTarget = (nonInclusiveId != -1);
            List<QTData> inRange = new List<QTData>();
            //List<T> outD = new List<T>();
            if (!_bounds.Intersects(range)) { return inRange; }

            for (int i = 0; i < _points.Count; i++)
            {
                if (range.ContainsPoint(_points[i]))
                {
                    if (excludeTarget) { if (_uids[i] == nonInclusiveId){continue;} }
                    inRange.Add(new QTData { indexedPos = _points[i], data = _dataList[i], found = true, uid = _uids[i]});
                }
            }

            if (_children.Count == 0) { return inRange; }

            inRange.AddRange(_children[0].GetPointsInRange(range));
            inRange.AddRange(_children[1].GetPointsInRange(range));
            inRange.AddRange(_children[2].GetPointsInRange(range));
            inRange.AddRange(_children[3].GetPointsInRange(range));

            return inRange;
        }
    }
}
